Поиск жёсткого преобразования облака точек

Ivan Ryzhikov
iwanryzij@yandex.ru
Mark Ilyasov

Постановка задачи

Даны два облака точек. Найти жёсткое преобразование из одного в другое

  • Соответвие между начальными и конечными точками не известно

  • Данные имеют шум

Note

Жёсткое преобразование сохраняет расстояние между точками, т.е. поворот + параллельный перенос

Более формально

Дано: \(X = \{ \mathbf{x}_1, \mathbf{x}_2, \dots, \mathbf{x}_N \} \subset \mathbb{R}^n\), \(Y = \{ \mathbf{y}_1, \mathbf{y}_2, \dots, \mathbf{y}_N \} \subset \mathbb{R}^n\)

\[ \mathbf{y}_{\pi(i)} = R \mathbf{x}_i + t + \boldsymbol{\epsilon}_i, \quad \forall i = 1, \dots, N, \]

где

  • \(\boldsymbol{\epsilon}_i \in \mathbb{R}^n\) — вектор шума для точки \(i\), \(\boldsymbol{\epsilon}_i \sim \mathcal{N}(0,\, (\sigma^2, \sigma^2,\sigma^2))\)

  • \(R \in SO(3)\) — матрица вращения \(R^T R = I\), \(\det(R) = 1\)

  • \(t \in \mathbb{R}^n\) — вектор смещения,

  • \(\pi: \{1, \dots, N\} \to \{1, \dots, N\}\) — перестановка индексов.

\[ \underset{R \in SO(3),\,t,\,\pi}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\pi(i)} + \boldsymbol{\epsilon}_i \|^2, \]

В терминах матриц

\[ Y^T = R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} + E, \]

  • \(\|\cdot\|_F\) — фробениусова норма,
  • \(R \in SO(3)\) — матрица вращения (\(R^T R = I\), \(\det(R) = 1\)),
  • \(P\) — матрица перестановки
    • \(P_{ij} \in \{0, 1\}\);
    • каждая строка и столбец имеют одну единицу — \(\forall i \sum_{j=1}^N P_{ij} = 1\), \(\forall j \sum_{i=1}^N P_{ij} = 1\)
  • \(E \in \mathbb{R}^{n \times N}\) — матрица шума с \(E[:, i] = \boldsymbol{\epsilon}_i\).

\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Проблема

\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^TP + t \begin{pmatrix} 1 & . . . & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Ключевая сложность — поиск матрицы соответствия.

Целевая функция приобретает дискретную форму — обычные методы оптимизации не применимы

Алгоритм Кабша(-Умеямы) умеет решать успрощенную задачу с известным соответствием

\[ \underset{R \in SO(3),\,t}{\arg \min}\ \| R X^T + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Решение

Избавимся от матрицы перестановок — будем уменьшать расстояние между облаками.

Изменим целевую функцию

\[ \underset{{R \in SO(3),\ t}}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\sigma(i)} \|^2 \]

  • \(\sigma(i)\) — индекс ближайшей точки \(\mathbf{y}_{\sigma(i)} \in Y\) для \(\mathbf{x}_i \in X\) 1

Идея алгоритма ICP

Iteartive Closest Point — итеративный метод поиска жёсткого преобразования через сопоставление ближайших точек. В качестве приближенного соответсвия точек используется сопоставление каждой ближайшей к ней (из другого облака). Для полученного соответствия можно использовать алгоритм Кабша

Этапы алгоритма ICP

  1. Инициализация начального преобразования
  2. Итерация цикла
    1. Применение текущего преобразования
    2. Построение соответствия между точками через поиск ближайшей
    3. Обновление приближения через алгоритм Кабша для найденного соответсвия
    4. Проверка сходимости алгоритма через метрику

Сходимость ICP

Каждую итерацию выбирается соответствие между ближайшими точками в облаках. Алгоритм Кабша уменьшает расстояние между этими точками. Целевая функция монотонно убывает (невозрастает). Таким образом, ICP приобретает локальную сходимость.

Применимость алгоритма к задаче

Минимум исходной метрики является минимумом так же и для целевой функции ICP

Подготовка к исследованию методов

Jupyter environment detected. Enabling Open3D WebVisualizer.
[Open3D INFO] WebRTC GUI backend enabled.
[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.

Генерация примеров (1/2)

Сферическое облако

Точки \(\mathbf{p}\) на сфере — результат нормирования случайного вектора \(\mathbf{v}\) с нормально-распределенными компонентами \(x\), \(y\) и \(z\), с последующим домножением на радиус \(r\) \[ \mathbf{p} = r \cdot \frac{\mathbf{v}}{\|\mathbf{v}\|} \] \[ \quad \mathbf{v} = (x, y, z) \sim \mathcal{N}(0, \sigma^2) \]

Генерация примеров (2/2)

Деформация сферы

Выраженно через воздействие вектора с силой обратно пропорциональной расстоянию до точки приложения

Исследование эффективности от угла

Генерирация целового облака

  • Поворот на заданное число вдоль плоскости XY
  • Наложение шума

Метрики эффективности

  • RMSE (среднеквадратическое расстояние от точек одного облака до другого облака)
  • Нормы Фробениуса от разности между найденной трансформацией и искомой

Исследование метода ICP

На всем дипазоне углов

Исследование метода ICP

Для углов от -45 до 45 градусов

Ограниченность ICP

Использование упрощенной метрики ведет к тому, что целевая функция преобретает значительное количество локальных минимумов. Кроме того, сопоставление точек облаков через ближайшие — довольно грубое приближение

Использование алгоритма ограничено

ICP показывает хорошие результаты только при малых углах поворота

Альтернативное решение

проблемы поиска соответствия

Поиск соответствий через инвариантные свойства

Соответствие можно установить на основе свойств, инвариантных к жёстким преобразованиям. Например:

  • Углы между нормалями к точкам
  • Расстояние до центра масс
  • Радиус кривизны

Упорядоченный набор таких свойств записывается как вектор, называемый дескриптором

Простейшее решение

Каждой точке облака сопоставим точку другого облака, ближайшую по дескрипторам. С появлением соответствия на основе дескрипторов сразу приходит простая и понятная идея - применить желанный алгоритм Кабша. Однако в реальной ситуации можно столкнуться с рядом трудностей

Что делать, если дескрипторы совпадают?

Простое решение - увеличить размер дескриптора! Больше инвариантных свойств — меньше вероятность совпадения ✅

И это решение хоть и кажется забаным, однако некоторые дескрипторы могут насчитывать более 2000 элементов.

Однако, это не всегда помогает. Из-за дубликатов может быть много неверных сопоставлений

Что делать, если дескрпитор далёк от всех дескрипторов другого облака?

Такая ситуация вполне может возникнуть если представить, что шум достаточное возмущение для того, чтобы испортить локальные метрики. В конце концов, можно найти некоторый ближайший дескриптор, однако, не будет ли столь плохое соответствие портить решение?

Решение. RANSAC

Что делать с возникающими несоответствиями? Исключать. Это довольно простой и изящный подход можно реализовать с помощью алгоритма RANSAC. В данном случае несоответствие можно вычислить через применение алгоритма Кабша - все пары точек, расстояние между которыми после оптимального поворота выше некоторого порога - исключаются как ложные соответствия. И цикл повторяется.

Исследование RANSAC

На всем дипазоне углов

5.912393093354067

Исследование RANSAC

Для углов от -45 до 45 градусов

5.912393093354067